/*
 * GslLeastSquaresSolver.cpp
 *
 * Copyright 2012 Pieter Agten
 *
 * This file is part of tltgen.
 *
 * tltgen is free software: you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 *
 * tltgen is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with tltgen.  If not, see <http://www.gnu.org/licenses/>.
 */

/**
 * @file   GslLeastSquaresSolver.cpp
 * @author Pieter Agten (pieter.agten@gmail.com)
 * @date   14 jan 2013
 */

#include <stdexcept>
#include <iostream>
#include <gsl/gsl_multifit_nlin.h>

#include "ThermistorTable.hpp"
#include "ThermistorModel.hpp"

#include "GslLeastSquaresSolver.hpp"

#define DEBUG false

namespace tltgen {

  GslLeastSquaresSolver::~GslLeastSquaresSolver()
  { }

  int GslLeastSquaresSolver::equation_f_wrapper(const gsl_vector *params, void *data, gsl_vector *f)
  {
    try {
      struct data* context = (struct data*)data;
      const double *paramsPtr = gsl_vector_const_ptr(params, 0);
      double *residuesPtr = gsl_vector_ptr(f, 0);

      context->model->applyModelFunction(context->thermistorTable, paramsPtr, residuesPtr);
      return GSL_SUCCESS;
    } catch (std::exception& e) {
      return GSL_EDOM;
    }
  }

  int GslLeastSquaresSolver::equation_df_wrapper(const gsl_vector *params, void *data, gsl_matrix *J)
  {
    try {
      struct data* context = (struct data*)data;
      const double *paramsPtr = gsl_vector_const_ptr(params, 0);
      double *derivativesPtr = gsl_matrix_ptr(J, 0, 0);

      context->model->applyModelDerivative(context->thermistorTable, paramsPtr, derivativesPtr);
      return GSL_SUCCESS;
    } catch (std::exception& e) {
      return GSL_EDOM;
    }
  }
  int GslLeastSquaresSolver::equation_fdf_wrapper(const gsl_vector *params, void *data, gsl_vector *f, gsl_matrix *J)
  {
    try {
      struct data* context = (struct data*)data;
      const double *paramsPtr = gsl_vector_const_ptr(params, 0);
      double *residuesPtr = gsl_vector_ptr(f, 0);
      double *derivativesPtr = gsl_matrix_ptr(J, 0, 0);

      context->model->applyModelFunction(context->thermistorTable, paramsPtr, residuesPtr);
      context->model->applyModelDerivative(context->thermistorTable, paramsPtr, derivativesPtr);
      return GSL_SUCCESS;
    } catch (std::exception& e) {
      return GSL_EDOM;
    }
  }

  void GslLeastSquaresSolver::calculateParameters(ThermistorModel* model, ThermistorTable* thermistorTable, double *result) const {
    if (! model->canBeSolved(thermistorTable)) {
      throw std::runtime_error("Generator is not ready to be solved.");
    }

    const size_t P = model->getNbParameters();
    const size_t N = thermistorTable->getNbValidRows();
    double guess[P];
    model->getInitialGuess(guess);
    gsl_vector_view initial_guess = gsl_vector_view_array(guess, P);

    /* Allocate the solver */
    const gsl_multifit_fdfsolver_type *solver_type = gsl_multifit_fdfsolver_lmsder;
    gsl_multifit_fdfsolver *solver = gsl_multifit_fdfsolver_alloc(solver_type, N, P);
    if (solver == NULL) {
      // TODO: Check for other errors? Register error handler?
      throw std::bad_alloc();
    }

    struct data context;
    context.model = model;
    context.thermistorTable = thermistorTable;

    /* Initialize the solver */
    gsl_multifit_function_fdf fdf;
    fdf.f = &equation_f_wrapper;
    fdf.df = &equation_df_wrapper;
    fdf.fdf = &equation_fdf_wrapper;
    fdf.n = N;
    fdf.p = P;
    fdf.params = (void *)&context;
    gsl_multifit_fdfsolver_set(solver, &fdf, &initial_guess.vector);


    /* Run the solver */
    int istatus;
    int tstatus;
    unsigned int iter = 0;
    do {
#if DEBUG
      std::cout << "[GSL solver] Starting iteration " << iter << "." << std::endl;
#endif
      iter += 1;
      istatus = gsl_multifit_fdfsolver_iterate(solver);
      tstatus = gsl_multifit_test_delta(solver->dx, solver->x, 1E-10, 1E-10);
    } while (istatus == GSL_SUCCESS && tstatus == GSL_CONTINUE && iter < getMaxNbIterations());
  
    if (tstatus != GSL_SUCCESS) {
      std::cerr << "[GSL solver] Failed to find parameters with requested precision after " << iter << " iterations: " << gsl_strerror(tstatus) << std::endl;
      gsl_multifit_fdfsolver_free(solver);
      throw std::runtime_error(gsl_strerror(tstatus));
    }
#if DEBUG
    std::cout << "[GSL solver] Found parameters with requested precision after " << iter << " iterations." << std::endl;
#endif

    /* Store the result */
    for(unsigned int i = 0; i < P; ++i) {
      result[i] = gsl_vector_get(solver->x, i);
    }

    /* Free the memory allocated for the solver */
    gsl_multifit_fdfsolver_free(solver);
#if DEBUG
    std::cout << "[GSL solver] Successfully completed paramter calculation." << std::endl;
#endif
  }

}

